/******************************************************************
编程环境：----------
        软件:Arduino2.3.4
        硬件:Mega2560主控板, Mega2560扩展版,3s电池盒，锂电池*3
             Windows11操作系统电脑，方口USB数据线*1，四自由度机械臂+机械爪
        库函数:<Arduino.h>, "uni_servo.h", "WonderK210.h"

实验目的：-----------
        1.了解如何使用视觉模块；
        2.搭建视觉模块+四自由度机械臂(+手爪)颜色分拣程序框架。

操作流程：----------
        详细请看操作视频.

实验现象：-----------
        当放置绿色物体时，机械臂抓取物体并放置到右侧，当放置蓝色物体时，机械
        臂抓取物体并放置到左侧.

实验接线简略图：----------
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
    电池仓         mega2560扩展版                机械臂
    12V母座~~~~~~~~~板载供电口1
    8.4V母座~~~~~~~~板载供电口2
                      4  ---------------------  爪子
                      6  ---------------------  手腕
                      7  ---------------------  小臂
                      10 ---------------------  大臂
                      11 ------------------ 机械臂底座转台
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
2024.12.19 Uni Tongchuang Intelligent Robot Technology CO.,Ltd
QQ交流群：811348489 （UNI机器人开源交流群）
*******************************************************************/
#include"WonderK210.h"
#include "uni_servo.h"

#define bord_rate 115200               //定义串口波特率
#define servo_num 5                    //定义舵机总数
#define servel_parts 100.0             //舵机需要转动的角度的细分数
#define servo_step_delay 15            //舵机每个细分数需要等待的时间
#define servo_action_delay_slowly 500  //机械臂两个动作需要等待的时间（慢）
#define servo_action_delay_quickly 500 //机械臂两个动作需要等待的时间（快）

//程序是否要进入调试模式，以及进入哪种调试模式，需要调试，把相应的声明打开即可
//#define DEBUG        //程序是否进入调试
//#define DEBUG_K210   //程序进入k210部分调试

UNI_SERVO_S servo(4, 6, 7, 10, 11);//声明舵机4,6,7,10,11处的舵机
//float servel_parts = 100.0;        //设定舵机舵机份数
int value_init_int[5] = {11, 100, 10, 82, 120};//舵机初始角度int类型
float value_init_float[servo_num] = {11.0, 100.0, 10.0, 82.0, 120.0};//舵机初始角度-float类型

WonderK210* wk;     //接收控制器
Find_Box_Msg_st* result;//消息存储变量
String recv_string = "";

void setup() {
  delay(1500);
  Serial.begin(bord_rate);
  servo.init();     //舵机引脚初始化
  k210_serial_init();   //k210串口初始化
  servo.move(11, 100, 10, 82, 120);delay(1000);
  Serial.println("judge_isnot_debug");delay(servo_action_delay_slowly);

  //debug section
  #ifdef DEBUG
    Serial.println("The program enters debugs modes");
    delay(servo_action_delay_slowly);
    while(1){
      #ifdef DEBUG_K210
        k210_test();
      #endif
    }
  #endif
  Serial.println("The program has not entered debugging mode");
  delay(servo_action_delay_slowly);
}

void loop() {
  //机械臂颜色分拣程序，当放置绿色物体时，机械臂抓取物体并放置到右侧，
  //当放置蓝色物体时，机械臂抓取物体并放置到左侧.
  color_detection();
}
